#pragma once
#ifndef ARMOR_PREDICT_H
#define ARMOR_PREDICT_H

#include <atomic>
#include <fstream>
#include <iostream>
#include <math.h>
#include <string>

#include "armor_kalman.hpp"
#include "../autoaim/ekf.h"
#include "../queues/circlequeue.h"
#include "../../../tool/include/types.h"
#include "../autoaim/tools.h"
// #include "../../../kinematic/include/kinematic/trajectory.hpp"
// TODO
constexpr int V_X = 8, V_Z = 3;
// constexpr int V_X = 6, V_Z = 3;
/**
 * @brief 装甲板运动预测器
 */
class ArmorPredictor
{
public:
  std::string param_path;
  int pre_state = 0;
  int post_state = 0;
  int64_t last_target_time = 0;//上次目标时间，单位是ms
  int64_t curr_target_time = 0;//当前目标时间，单位是ms
  
  int width = 1280;
  int height = 960;
  cv::Mat cameraMatrix;
  cv::Mat distCoeffs;
  cv::Mat offset;//画图用到的数据

  /// 用于debug的image
  cv::Mat canvas;

  /// 运动描述体
  ArmorMotionDescriptior descriptior;

  /// circe queue to record one armor history
  CircleQueue<ArmorMotionDescriptior, 300> history_descriptiors;

  /// 线性卡尔曼滤波器
  Kalman<V_X, V_Z> kalman;

  autoaim_ekf<9, 4> ekf;
  int length_of_filter = 4;
  autoaim_filter<double> sin_o_yaw_filter;
  autoaim_filter<double> cos_o_yaw_filter;
  circularQueue<Target> target_queue;
  circularQueue<double> queue_ekf_omega; 
  BallisitcSolver ballistic_solver;
  double ekf_yaw_v = 0;

public:
  void load_param_path(const std::string& param_path)
  {
    this->param_path = param_path;
    ekf.load_param_path(param_path);
  }
  /**
   * @brief 析构函数
   *
   */
  ArmorPredictor()
  {
    target_queue.init(3);
    queue_ekf_omega.init(100);
  }
  ~ArmorPredictor();

  /**
   * @brief 更新运动描述器
   */
  void update(Target target, double v, bool flag_switch_armor);

  /**
   * @brief 判断预测器是否具有实时性
   * @details RT : real-time
   */
  bool if_predictor_RT(int64_t predictor_last_update_time_stamp, int64_t clock_no);

  /**
   * @brief 为了防止数据跳动,做一下滤波
   * @details 如果数据点与运动拟合的数据点相差过大, cnt增1, cnt不超出阈值, 则丢掉
   * @details cnt超出阈值, 将历史点清空,并重新拟合
   * @returns 0:丢掉点 1:接受点 2:把历史点清空
   */
  int judgeInComePointAcceptable(Target &target, double v, int64_t image_clock);

  /**
   * @brief 获得大地系中的预测三维点
   */
  void predTarget(double v, int64_t clock_now, SendPack &send_pack);

  void ekf_predTarget(const double& bullet_speed, const int64_t& clock_now, SendPack& send_pack,const double& hit_range, const bool& static_hit, const double& static_hit_range);

  /**
   * @brief 在canvas上显示信息
   */
  void displayInfo();

  /**
   * @brief
   *
   */
  void show_pred(cv::Mat &canvas, const Target &pre, const Target &post, const Pose &pose, double v);

  /**
   * @brief compute past bullet fall point at this moment
   */
  bool getBulletFallPoint(int64_t clock_now, double bullet_speed, Target &fall_point);

  /**
   * @brief 是否爆发发弹
   */
  bool burst_shoot(const Pose &pose, double v, cv::RotatedRect &rect);

  /**
   * @brief 热调参
   *
   */
  void tune_param();
  void get_tracker_state(const int& pre_state, const int& post_state);
};

#endif